using BepuUtilities.Memory;
using System;
using System.Diagnostics;
using BepuUtilities;
using System.Runtime.CompilerServices;
using BepuPhysics.Constraints;
using BepuPhysics.Collidables;
using BepuPhysics.CollisionDetection;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif

namespace BepuPhysics
{
    public struct BodyMemoryLocation
    {
        /// <summary>
        /// 拥有正文引用的集合的索引。如果孤岛索引为0,则实体处于活动状态。
        /// </summary>
        public int SetIndex;
        /// <summary>
        /// 其拥有集内的正文索引。如果Body处于活动状态(因此Island索引为-1),则这是Body数据数组的索引。
        /// 如果它不是负数,则它是指向非活动岛的索引
        /// </summary>
        public int Index;
    }

    /// <summary>
    /// 所有分配的主体的集合。
    /// </summary>
    public class Bodies
    {
        /// <summary>
        /// 将正文句柄整数值重新映射到正文的实际数组索引。
        /// 后备阵列索引可以响应于高速缓存优化而改变。
        /// </summary>
        public Buffer<BodyMemoryLocation> HandleToLocation;
        public IdPool HandlePool;
        /// <summary>
        /// 现有实体的集合。索引0处的槽包含所有活动实体。稍后的插槽(如果分配)包含与非活动岛相关联的主体。
        /// 请注意,此缓冲区不一定包含连续元素。删除集合时,会保留间隙。
        /// </summary>
        public Buffer<BodySet> Sets;
        /// <summary>
        /// 获取对活动集的引用,该活动集存储在集缓冲区的索引0中。
        /// </summary>
        /// <return>对活动正文集的引用。</return>
        public unsafe ref BodySet ActiveSet { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return ref *Sets.Memory; } }

        // 待办事项：鉴于念力的昙花一现,将惰性公之于众似乎是新浪微博的秘诀。我们可能希望在帧执行之后显式删除它,并且
        // 千万不要曝光它。如果用户真的想要一个最新的世界空间惯性,对他们来说,无论如何都很容易从当地的惯性和方向来构建它。
        /// <summary>
        /// 自上次更新以来,世界改变了主动体的惯性。请注意,对于直接方向更改或身体记忆移动,此选项不会自动更新。
        /// 它在帧期间只更新一次。它应该被视为短暂的信息。
        /// </summary>
        public Buffer<BodyInertia> Inertias;
        public BufferPool Pool { get; private set; }

        internal IslandAwakener awakener;
        internal IslandSleeper sleeper;
        internal Shapes shapes;
        internal BroadPhase broadPhase;
        internal Solver solver;

        /// <summary>
        /// 获取或设置每个实体的最小约束容量。未来的大小调整或分配将遵循此最小值,但更改此值不会立即调整现有列表的大小。
        /// </summary>
        public int MinimumConstraintCapacityPerBody { get; set; }


        /// <summary>
        /// 构造新的Body集合。必须调用Initialize才能使实例准备好使用。
        /// </summary>
        /// <param name="pool">要从中提取永久分配的集合的池。</param>
        /// <param name="Shares">集合正文引用的形状。</param>
        /// <param name="宽相位">包含正文可碰撞项的宽相位。</param>
        /// <param name="initialBodyCapacity">要在活动集中为其分配空间的初始正文数。</param>
        /// <param name="initialIslandCapacity">要在集缓冲区中为其分配空间的岛的初始数量。</param>
        /// <param name="initialConstraintCapacityPerBody">Expected要为其分配空间的每个正文的约束引用数。</param>
        public unsafe Bodies(BufferPool pool, Shapes shapes, BroadPhase broadPhase,
            int initialBodyCapacity, int initialIslandCapacity, int initialConstraintCapacityPerBody)
        {
            this.Pool = pool;

            // 请注意,id池仅在删除时才会增长,因此这只是一个启发式初始化。
            // 你可以用一些不那么激进的东西,但它确实倾向于避免在极端波动的情况下调整大小。
            HandlePool = new IdPool(initialBodyCapacity, pool);
            ResizeHandles(initialBodyCapacity);
            ResizeSetsCapacity(initialIslandCapacity + 1, 0);
            ActiveSet = new BodySet(initialBodyCapacity, pool);
            this.shapes = shapes;
            this.broadPhase = broadPhase;
            MinimumConstraintCapacityPerBody = initialConstraintCapacityPerBody;
        }

        /// <summary>
        /// 初始化实体集。用于完成双向依赖关系。
        /// </summary>
        /// <param name="solver">负责连接到集合体的约束的求解器。</param>
        /// <param name="wakener">孤岛唤醒程序在主体经历要求其存在于活动集中的过渡时使用。</param>
        public void Initialize(Solver solver, IslandAwakener awakener, IslandSleeper sleeper)
        {
            this.solver = solver;
            this.awakener = awakener;
            this.sleeper = sleeper;
        }

        /// <summary>
        /// 更新身体当前状态的宽阶段内保持的边界。不按速度扩展边界框。如果没有与躯干相关联的形状,则此操作不会执行任何操作。
        /// </summary>
        public void UpdateBounds(BodyHandle bodyHandle)
        {
            ref var location = ref HandleToLocation[bodyHandle.Value];
            ref var set = ref Sets[location.SetIndex];
            ref var collidable = ref set.Collidables[location.Index];
            if (collidable.Shape.Exists)
            {
                shapes.UpdateBounds(set.Poses[location.Index], ref collidable.Shape, out var bodyBounds);
                if (location.SetIndex == 0)
                {
                    broadPhase.UpdateActiveBounds(collidable.BroadPhaseIndex, bodyBounds.Min, bodyBounds.Max);
                }
                else
                {
                    broadPhase.UpdateStaticBounds(collidable.BroadPhaseIndex, bodyBounds.Min, bodyBounds.Max);
                }
            }

        }

        void AddCollidableToBroadPhase(BodyHandle bodyHandle, in RigidPose pose, in BodyInertia localInertia, ref Collidable collidable)
        {
            Debug.Assert(collidable.Shape.Exists);
            // 这个物体有一个可碰撞的;把它贴在宽相中。
            // 请注意,我们必须为宽相位计算一个初始边界框,以便能够有效地插入它。
            // (在批量添加的情况下,您将希望使用批处理AABB计算或只使用缓存值。)
            // 注意：这里的最小和最大值以绝对坐标表示,这意味着在位置使用更高精度表示的情况下,这是一个必须更新的点。
            shapes.UpdateBounds(pose, ref collidable.Shape, out var bodyBounds);
            // 请注意,始终假定新实体可碰撞对象处于活动状态。
            collidable.BroadPhaseIndex =
                broadPhase.AddActive(
                    new CollidableReference(IsKinematic(localInertia) ? CollidableMobility.Kinematic : CollidableMobility.Dynamic, bodyHandle),
                    ref bodyBounds);
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        internal void UpdateCollidableBroadPhaseIndex(BodyHandle handle, int newBroadPhaseIndex)
        {
            ref var movedOriginalLocation = ref HandleToLocation[handle.Value];
            Sets[movedOriginalLocation.SetIndex].Collidables[movedOriginalLocation.Index].BroadPhaseIndex = newBroadPhaseIndex;
        }
        void RemoveCollidableFromBroadPhase(ref Collidable collidable)
        {
            var removedBroadPhaseIndex = collidable.BroadPhaseIndex;
            // 下图将物体的可碰撞部分从宽幅相位中移除,并调整任何移动的树叶的宽幅相位指数。
            if (broadPhase.RemoveActiveAt(removedBroadPhaseIndex, out var movedLeaf))
            {
                // 请注意,这始终是一个主动的身体,所以我们知道,在宽广的阶段中取代身体位置的东西也是一个主动的身体。
                // 所有静态体和静止体都存在于静电树中。
                Debug.Assert(movedLeaf.Mobility != CollidableMobility.Static);
                UpdateCollidableBroadPhaseIndex(movedLeaf.BodyHandle, removedBroadPhaseIndex);
            }
        }
        /// <summary>
        /// 将新的活动实体添加到模拟中。
        /// </summary>
        /// <param name="description">要添加的正文的说明。</param>
        /// <return>创建的正文的句柄。</return>
        public unsafe BodyHandle Add(in BodyDescription description)
        {
            Debug.Assert(HandleToLocation.Allocated, "The backing memory of the bodies set should be initialized before use.");
            var handleIndex = HandlePool.Take();
            Debug.Assert(handleIndex <= HandleToLocation.Length, "It should be impossible for a new handle to end up more than one slot beyond the current handle to index array. " +
                "This would imply some form of resize or compaction bug.");
            if (handleIndex == HandleToLocation.Length)
            {
                // 超出空间;需要调整大小。
                ResizeHandles(HandleToLocation.Length << 1);
            }
            Debug.Assert(Math.Abs(description.Pose.Orientation.Length() - 1) < 1e-6f, "Orientation should be initialized to a unit length quaternion.");

            // 为简单起见,所有新实体都处于活动状态。总有一天,为不活跃的人提供一条优化的路径可能是值得的,但它会增加复杂性。
            // (在某些联网的开放世界方案中,直接添加非活动实体可能会很有帮助。)
            var handle = new BodyHandle(handleIndex);
            var index = ActiveSet.Add(description, handle, MinimumConstraintCapacityPerBody, Pool);
            HandleToLocation[handleIndex] = new BodyMemoryLocation { SetIndex = 0, Index = index };

            if (description.Collidable.Shape.Exists)
            {
                AddCollidableToBroadPhase(handle, description.Pose, description.LocalInertia, ref ActiveSet.Collidables[index]);
            }
            return handle;
        }

        internal BodyHandle RemoveFromActiveSet(int activeBodyIndex)
        {
            // 请注意,由于休眠,这与主拆卸是分开的。休眠并不想真正从*模拟中删除,只想从活动集中删除。
            // 约束以及对约束的引用保持不变。
            ref var set = ref ActiveSet;
            Debug.Assert(activeBodyIndex >= 0 && activeBodyIndex < set.Count);
            ValidateExistingHandle(set.IndexToHandle[activeBodyIndex]);
            ref var collidable = ref set.Collidables[activeBodyIndex];
            if (collidable.Shape.Exists)
            {
                // 由于存在可碰撞物质,因此应将其从宽相中去除。
                // 即使在睡眠环境中使用此函数时也是如此。可碰撞的将被读取到静电树上。
                RemoveCollidableFromBroadPhase(ref collidable);
            }

            var bodyMoved = set.RemoveAt(activeBodyIndex, out var handle, out var movedBodyIndex, out var movedBodyHandle);
            if (bodyMoved)
            {
                // 虽然被移除的实体没有任何与其关联的约束,但移动以填充其插槽的实体可能会！
                solver.UpdateForBodyMemoryMove(movedBodyIndex, activeBodyIndex);
                Debug.Assert(HandleToLocation[movedBodyHandle.Value].SetIndex == 0 && HandleToLocation[movedBodyHandle.Value].Index == movedBodyIndex);
                HandleToLocation[movedBodyHandle.Value].Index = activeBodyIndex;
            }
            return handle;

        }
        /// <summary>
        /// 按索引移除活动实体。任何连接到此实体的约束都将被删除。假定输入位置有效。
        /// </summary>
        /// <param name="activeBodyIndex">活动正文的索引。</param>
        public void RemoveAt(int activeBodyIndex)
        {
            // 必须删除约束;我们不能将"孤儿"留在求解器中,因为它们将访问无效数据。
            ref var constraints = ref ActiveSet.Constraints[activeBodyIndex];
            for (int i = constraints.Count - 1; i >= 0; --i)
            {
                solver.Remove(constraints[i].ConnectingConstraintHandle);
            }
            constraints.Dispose(Pool);

            var handle = RemoveFromActiveSet(activeBodyIndex);

            HandlePool.Return(handle.Value, Pool);
            ref var removedBodyLocation = ref HandleToLocation[handle.Value];
            removedBodyLocation.SetIndex = -1;
            removedBodyLocation.Index = -1;
        }

        /// <summary>
        /// 通过实体的控制柄将其从集合中移除。如果实体处于非活动状态,则其孤岛中的所有实体都将强制处于活动状态。
        /// </summary>
        /// <param name="Handle">要删除的正文句柄。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Remove(BodyHandle handle)
        {
            ValidateExistingHandle(handle);
            awakener.AwakenBody(handle);
            RemoveAt(HandleToLocation[handle.Value].Index);
        }

        /// <summary>
        /// 将约束添加到激活实体的约束列表。
        /// </summary>
        /// <param name="bodyIndex">要向其添加约束的正文的索引。</param>
        /// <param name="constraintHandle">要添加的约束的句柄。</param>
        /// <param name="indexInConstraint">约束中正文的索引。</param>
        internal void AddConstraint(int bodyIndex, ConstraintHandle constraintHandle, int indexInConstraint)
        {
            ActiveSet.AddConstraint(bodyIndex, constraintHandle, indexInConstraint, Pool);
        }

        /// <summary>
        /// 从激活实体的约束列表中移除约束。
        /// </summary>
        /// <param name="bodyIndex">活动正文的索引。</param>
        /// <param name="constraintHandle">要删除的约束的句柄。</param>
        internal void RemoveConstraintReference(int bodyIndex, ConstraintHandle constraintHandle)
        {
            ActiveSet.RemoveConstraintReference(bodyIndex, constraintHandle, MinimumConstraintCapacityPerBody, Pool);
        }

        /// <summary>
        /// 获取惯性是否与运动学实体的惯性匹配(即,所有反向质量和惯性分量均为零)。
        /// </summary>
        /// <param name="惯性">要分析的身体惯性。</param>
        /// <return>如果质量和惯性成反比的所有分量都为零,则为True,否则为False。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool IsKinematic(in BodyInertia inertia)
        {
            return inertia.InverseMass == 0 && HasLockedInertia(inertia.InverseInertiaTensor);
        }

        /// <summary>
        /// 获取角惯量是否与运动学实体的角惯性匹配(即,所有反惯性张量分量均为零)。
        /// </summary>
        /// <param name="惯性">要分析的身体惯性。</param>
        /// <return>如果质量和惯性成反比的所有分量都为零,则为True,否则为False。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool HasLockedInertia(in Symmetric3x3 inertia)
        {
            return inertia.XX == 0 &&
                   inertia.YX == 0 &&
                   inertia.YY == 0 &&
                   inertia.ZX == 0 &&
                   inertia.ZY == 0 &&
                   inertia.ZZ == 0;
        }

        private struct ConnectedDynamicCounter : IForEach<int>
        {
            public Bodies Bodies;
            public int DynamicCount;
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public void LoopBody(int bodyIndex)
            {
                // 解算器的连接实体枚举直接提供约束存储的引用,该引用是活动约束的活动集中的索引和非活动约束的句柄。
                // 我们在"成为运动学"的开始就强迫动力学是活跃的,所以我们不必担心事物不活跃的一面。
                if (!IsKinematic(Bodies.ActiveSet.LocalInertias[bodyIndex]))
                    ++DynamicCount;
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        void UpdateForKinematicStateChange(BodyHandle handle, ref BodyMemoryLocation location, ref BodySet set, bool newlyKinematic)
        {
            Debug.Assert(location.SetIndex == 0, "If we're changing kinematic state, we should have already awoken the body.");
            ref var collidable = ref set.Collidables[location.Index];
            if (collidable.Shape.Exists)
            {
                var mobility = IsKinematic(set.LocalInertias[location.Index]) ? CollidableMobility.Kinematic : CollidableMobility.Dynamic;
                if (location.SetIndex == 0)
                {
                    broadPhase.activeLeaves[collidable.BroadPhaseIndex] = new CollidableReference(mobility, handle);
                }
                else
                {
                    broadPhase.staticLeaves[collidable.BroadPhaseIndex] = new CollidableReference(mobility, handle);
                }
            }
            if (newlyKinematic)
            {
                ref var constraints = ref set.Constraints[location.Index];
                ConnectedDynamicCounter enumerator;
                enumerator.Bodies = this;
                for (int i = 0; i < constraints.Count; ++i)
                {
                    ref var constraint = ref constraints[i];
                    enumerator.DynamicCount = 0;
                    solver.EnumerateConnectedBodies(constraint.ConnectingConstraintHandle, ref enumerator);
                    if (enumerator.DynamicCount == 0)
                    {
                        // 此约束仅连接运动学实体;将其保留在解算器中会导致奇点。
                        solver.Remove(constraint.ConnectingConstraintHandle);
                    }
                }
            }
        }

        /// <summary>
        /// 更改与物体关联的局部质量和惯性张量。正确处理运动学和动力学之间的过渡。
        /// 如果实体变为运动学的,则只包含运动学实体的任何约束都将被删除。
        /// 唤醒身体。
        /// </summary>
        /// <param name="Handle">应更改惯性的实体的句柄。</param>
        /// <param name="localInertia">身体的新局部惯性。</param>
        /// <remarks>
        /// 仅当惯性更改可能会导致动力学状态和运动学状态之间的转换时,才需要此功能。
        /// 如果保证改变前后是动态的,就可以不出问题地直接修改惯性。
        /// </remarks>
        public void SetLocalInertia(BodyHandle handle, in BodyInertia localInertia)
        {
            ref var location = ref HandleToLocation[handle.Value];
            if (location.SetIndex > 0)
            {
                // 身体是不活动的。把它叫醒。
                awakener.AwakenBody(handle);
            }
            // 请注意,HandleToLocation插槽引用仍然有效;它可能已经更新,但句柄插槽不会移动。
            ref var set = ref Sets[location.SetIndex];
            var newlyKinematic = IsKinematic(localInertia) && !IsKinematic(set.LocalInertias[location.Index]);
            set.LocalInertias[location.Index] = localInertia;
            UpdateForKinematicStateChange(handle, ref location, ref set, newlyKinematic);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        void UpdateForShapeChange(BodyHandle handle, int activeBodyIndex, TypedIndex oldShape, TypedIndex newShape)
        {
            if (oldShape.Exists != newShape.Exists)
            {
                ref var set = ref ActiveSet;
                if (newShape.Exists)
                {
                    // 将可碰撞对象添加到新形状的模拟中。
                    AddCollidableToBroadPhase(handle, set.Poses[activeBodyIndex], set.LocalInertias[activeBodyIndex], ref set.Collidables[activeBodyIndex]);
                }
                else
                {
                    // 从模拟中移除现在未使用的可碰撞对象。
                    RemoveCollidableFromBroadPhase(ref set.Collidables[activeBodyIndex]);
                }
            }
        }
        /// <summary>
        /// 更改实体的形状。正确处理无形状和有形状之间的过渡。如果身体不活动,它将被强迫唤醒。
        /// 在宽阔阶段更新躯干的边界。
        /// </summary>
        /// <param name="Handle">要更改其形状的正文句柄。</param>
        /// <param name="newShape">要用于正文的新形状的索引。</param>
        public void SetShape(BodyHandle handle, TypedIndex newShape)
        {
            ref var location = ref HandleToLocation[handle.Value];
            if (location.SetIndex > 0)
            {
                // 身体是不活动的。把它叫醒。
                awakener.AwakenBody(handle);
            }
            // 请注意,HandleToLocation插槽引用仍然有效;它可能已经更新,但句柄插槽不会移动。
            Debug.Assert(location.SetIndex == 0, "We should be working with an active shape.");
            ref var set = ref ActiveSet;
            ref var collidable = ref set.Collidables[location.Index];
            var oldShape = collidable.Shape;
            collidable.Shape = newShape;
            UpdateForShapeChange(handle, location.Index, oldShape, newShape);
            UpdateBounds(handle);
        }

        /// <summary>
        /// 将描述应用于实体。正确处理动力学和运动学之间以及无形状和有形状之间的任何过渡。
        /// 如果实体变为运动学的,则只包含运动学实体的任何约束都将被删除。唤醒身体。
        /// 在宽阔阶段更新躯干的边界。
        /// </summary>
        /// <param name="Handle">要接收说明的正文句柄。</param>
        /// <param name="description">要应用于正文的说明。</param>
        public void ApplyDescription(BodyHandle handle, in BodyDescription description)
        {
            ValidateExistingHandle(handle);
            ref var location = ref HandleToLocation[handle.Value];
            if (location.SetIndex > 0)
            {
                // 身体是不活动的。把它叫醒。
                awakener.AwakenBody(handle);
            }
            // 请注意,HandleToLocation插槽引用仍然有效;它可能已经更新,但句柄插槽不会移动。
            ref var set = ref Sets[location.SetIndex];
            ref var collidable = ref set.Collidables[location.Index];
            var oldShape = collidable.Shape;
            var newlyKinematic = IsKinematic(description.LocalInertia) && !IsKinematic(set.LocalInertias[location.Index]);
            set.ApplyDescriptionByIndex(location.Index, description);
            UpdateForShapeChange(handle, location.Index, oldShape, description.Collidable.Shape);
            UpdateForKinematicStateChange(handle, ref location, ref set, newlyKinematic);
            UpdateBounds(handle);
        }

        /// <summary>
        /// 按句柄获取正文的说明。
        /// </summary>
        /// <param name="Handle">要查找的正文的句柄。</param>
        /// <param name="description">正文的说明。</param>
        public void GetDescription(BodyHandle handle, out BodyDescription description)
        {
            ValidateExistingHandle(handle);
            ref var location = ref HandleToLocation[handle.Value];
            ref var set = ref Sets[location.SetIndex];
            set.GetDescription(location.Index, out description);
        }

        /// <summary>
        /// 通过句柄获取对Body的引用。
        /// </summary>
        /// <param name="Handle">要获取其引用的正文的句柄。</param>
        /// <return>对所需正文的引用。</return>
        public BodyReference GetBodyReference(BodyHandle handle)
        {
            ValidateExistingHandle(handle);
            return new BodyReference(handle, this);
        }

        /// <summary>
        /// 检查当前是否使用主体集注册了主体句柄。
        /// </summary>
        /// <param name="bodyHandle">要检查的句柄。</param>
        /// <return>如果句柄存在于集合中,则为True,否则为False。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public bool BodyExists(BodyHandle bodyHandle)
        {
            // 负集索引将正文句柄标记为未使用。
            return bodyHandle.Value >= 0 && bodyHandle.Value < HandleToLocation.Length && HandleToLocation[bodyHandle.Value].SetIndex >= 0;
        }

        [Conditional("DEBUG")]
        internal void ValidateExistingHandle(BodyHandle handle)
        {
            Debug.Assert(handle.Value >= 0, "Handles must be nonnegative.");
            Debug.Assert(handle.Value <= HandlePool.HighestPossiblyClaimedId && HandlePool.HighestPossiblyClaimedId < HandleToLocation.Length,
                "Existing handles must fit within the body handle->index mapping.");
            ref var location = ref HandleToLocation[handle.Value];
            Debug.Assert(location.SetIndex >= 0 && location.SetIndex < Sets.Length, "Body set index must be nonnegative and within the sets buffer length.");
            ref var set = ref Sets[location.SetIndex];
            Debug.Assert(set.Allocated);
            Debug.Assert(set.Count <= set.IndexToHandle.Length);
            Debug.Assert(location.Index >= 0 && location.Index < set.Count, "Body index must fall within the existing body set.");
            Debug.Assert(set.IndexToHandle[location.Index].Value == handle.Value, "Handle->index must match index->handle map.");
            Debug.Assert(BodyExists(handle), "Body must exist according to the BodyExists test.");
        }

        [Conditional("CHECKMATH")]
        internal void ValidateMotionStates()
        {
            for (int i = 0; i < Sets.Length; ++i)
            {
                ref var set = ref Sets[i];
                if (set.Allocated)
                {
                    for (int j = 0; j < set.Count; ++j)
                    {
                        ref var pose = ref set.Poses[j];
                        ref var velocity = ref set.Velocities[j];
                        try
                        {
                            pose.Position.Validate();
                            pose.Orientation.ValidateOrientation();
                            velocity.Linear.Validate();
                            velocity.Angular.Validate();
                        }
                        catch
                        {
                            Console.WriteLine($"Validation failed on body {i} of set {j}. Position: {pose.Position}, orientation: {pose.Orientation}, linear: {velocity.Linear}, angular: {velocity.Angular}");
                            throw;
                        }

                    }
                }
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static void GatherInertiaForBody(ref BodyInertia source, ref BodyInertias targetSlot)
        {
            GatherScatter.GetFirst(ref targetSlot.InverseInertiaTensor.XX) = source.InverseInertiaTensor.XX;
            GatherScatter.GetFirst(ref targetSlot.InverseInertiaTensor.YX) = source.InverseInertiaTensor.YX;
            GatherScatter.GetFirst(ref targetSlot.InverseInertiaTensor.YY) = source.InverseInertiaTensor.YY;
            GatherScatter.GetFirst(ref targetSlot.InverseInertiaTensor.ZX) = source.InverseInertiaTensor.ZX;
            GatherScatter.GetFirst(ref targetSlot.InverseInertiaTensor.ZY) = source.InverseInertiaTensor.ZY;
            GatherScatter.GetFirst(ref targetSlot.InverseInertiaTensor.ZZ) = source.InverseInertiaTensor.ZZ;
            GatherScatter.GetFirst(ref targetSlot.InverseMass) = source.InverseMass;
        }

        /// <summary>
        /// 将一个躯干捆绑的惯性聚集到AOSOA捆绑中。
        /// </summary>
        /// <param name="References">正在收集活动正文索引。</param>
        /// <param name="count">捆绑中的正文数。</param>
        /// <param name="惯性A">收集身体A的惯性</param>
        /// <param name="inetiaB">收集身体B的惯性</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void GatherInertia(ref Vector<int> references, int count,
            out BodyInertias inertiaA)
        {
            inertiaA = default;
            Debug.Assert(count >= 0 && count <= Vector<float>.Count);
            // 抓取身体指数的基本引用。请注意,我们再次使用了引用内存布局。
            ref var baseIndexA = ref Unsafe.As<Vector<int>, int>(ref references);
            for (int i = 0; i < count; ++i)
            {
                GatherInertiaForBody(ref Inertias[Unsafe.Add(ref baseIndexA, i)], ref GatherScatter.GetOffsetInstance(ref inertiaA, i));
            }
        }

        /// <summary>
        /// 将两个躯干束的惯性收集到AOSOA束中。
        /// </summary>
        /// <param name="References">正在收集活动正文索引。</param>
        /// <param name="count">捆绑中的正文数。</param>
        /// <param name="惯性A">收集身体A的惯性</param>
        /// <param name="inetiaB">收集身体B的惯性</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void GatherInertia(ref TwoBodyReferences references, int count, out BodyInertias inertiaA, out BodyInertias inertiaB)
        {
            inertiaA = default; inertiaB = default;
            Debug.Assert(count >= 0 && count <= Vector<float>.Count);
            // 抓取身体指数的基本引用。请注意,我们再次使用了引用内存布局。
            ref var baseIndexA = ref Unsafe.As<Vector<int>, int>(ref references.IndexA);
            ref var baseIndexB = ref Unsafe.As<Vector<int>, int>(ref references.IndexB);
            for (int i = 0; i < count; ++i)
            {
                GatherInertiaForBody(ref Inertias[Unsafe.Add(ref baseIndexA, i)], ref GatherScatter.GetOffsetInstance(ref inertiaA, i));
                GatherInertiaForBody(ref Inertias[Unsafe.Add(ref baseIndexB, i)], ref GatherScatter.GetOffsetInstance(ref inertiaB, i));
            }
        }

        /// <summary>
        /// 将三个车身束的惯性收集到AOSOA束中。
        /// </summary>
        /// <param name="References">正在收集活动正文索引。</param>
        /// <param name="count">捆绑中的正文数。</param>
        /// <param name="惯性A">收集身体A的惯性</param>
        /// <param name="inetiaB">收集身体B的惯性</param>
        /// <param name="inetiaC">收集身体C的惯性。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void GatherInertia(ref ThreeBodyReferences references, int count, out BodyInertias inertiaA, out BodyInertias inertiaB, out BodyInertias inertiaC)
        {
            inertiaA = default; inertiaB = default; inertiaC = default;
            Debug.Assert(count >= 0 && count <= Vector<float>.Count);
            // 抓取身体指数的基本引用。请注意,我们再次使用了引用内存布局。
            ref var baseIndexA = ref Unsafe.As<Vector<int>, int>(ref references.IndexA);
            ref var baseIndexB = ref Unsafe.As<Vector<int>, int>(ref references.IndexB);
            ref var baseIndexC = ref Unsafe.As<Vector<int>, int>(ref references.IndexC);
            for (int i = 0; i < count; ++i)
            {
                GatherInertiaForBody(ref Inertias[Unsafe.Add(ref baseIndexA, i)], ref GatherScatter.GetOffsetInstance(ref inertiaA, i));
                GatherInertiaForBody(ref Inertias[Unsafe.Add(ref baseIndexB, i)], ref GatherScatter.GetOffsetInstance(ref inertiaB, i));
                GatherInertiaForBody(ref Inertias[Unsafe.Add(ref baseIndexC, i)], ref GatherScatter.GetOffsetInstance(ref inertiaC, i));
            }
        }

        /// <summary>
        /// 将四个身体束的惯性收集到AOSOA束中。
        /// </summary>
        /// <param name="References">正在收集活动正文索引。</param>
        /// <param name="count">捆绑中的正文数。</param>
        /// <param name="惯性A">收集身体A的惯性</param>
        /// <param name="inetiaB">收集身体B的惯性</param>
        /// <param name="inetiaC">收集身体C的惯性。</param>
        /// <param name="inetiaD">收集身体D的惯性</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void GatherInertia(ref FourBodyReferences references, int count, out BodyInertias inertiaA, out BodyInertias inertiaB, out BodyInertias inertiaC, out BodyInertias inertiaD)
        {
            inertiaA = default; inertiaB = default; inertiaC = default; inertiaD = default;
            Debug.Assert(count >= 0 && count <= Vector<float>.Count);
            // 抓取身体指数的基本引用。请注意,我们再次使用了引用内存布局。
            ref var baseIndexA = ref Unsafe.As<Vector<int>, int>(ref references.IndexA);
            ref var baseIndexB = ref Unsafe.As<Vector<int>, int>(ref references.IndexB);
            ref var baseIndexC = ref Unsafe.As<Vector<int>, int>(ref references.IndexC);
            ref var baseIndexD = ref Unsafe.As<Vector<int>, int>(ref references.IndexD);
            for (int i = 0; i < count; ++i)
            {
                GatherInertiaForBody(ref Inertias[Unsafe.Add(ref baseIndexA, i)], ref GatherScatter.GetOffsetInstance(ref inertiaA, i));
                GatherInertiaForBody(ref Inertias[Unsafe.Add(ref baseIndexB, i)], ref GatherScatter.GetOffsetInstance(ref inertiaB, i));
                GatherInertiaForBody(ref Inertias[Unsafe.Add(ref baseIndexC, i)], ref GatherScatter.GetOffsetInstance(ref inertiaC, i));
                GatherInertiaForBody(ref Inertias[Unsafe.Add(ref baseIndexD, i)], ref GatherScatter.GetOffsetInstance(ref inertiaD, i));
            }
        }

        /// <summary>
        /// 将两个主体束的方向收集到AOSOA束中。
        /// </summary>
        /// <param name="References">正在收集活动正文索引。</param>
        /// <param name="count">捆绑中的正文对数量。</param>
        /// <param name="OrientationA">收集的正文A的方向</param>
        /// <param name="OrientationB">收集的正文B的方向</param>
        // [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void GatherOrientation(ref TwoBodyReferences references, int count,
            out QuaternionWide orientationA, out QuaternionWide orientationB)
        {
            orientationA = default; orientationB = default;
            Debug.Assert(count >= 0 && count <= Vector<float>.Count);
            // 抓取身体指数的基本引用。请注意,我们再次使用了引用内存布局。
            ref var baseIndexA = ref Unsafe.As<Vector<int>, int>(ref references.IndexA);
            ref var baseIndexB = ref Unsafe.As<Vector<int>, int>(ref references.IndexB);

            ref var poses = ref ActiveSet.Poses;
            for (int i = 0; i < count; ++i)
            {
                ref var indexA = ref Unsafe.Add(ref baseIndexA, i);
                QuaternionWide.WriteFirst(poses[indexA].Orientation, ref GatherScatter.GetOffsetInstance(ref orientationA, i));

                ref var indexB = ref Unsafe.Add(ref baseIndexB, i);
                QuaternionWide.WriteFirst(poses[indexB].Orientation, ref GatherScatter.GetOffsetInstance(ref orientationB, i));
            }
        }

        /// <summary>
        /// 将One Body捆绑包的方向收集到AOSOA捆绑包中。
        /// </summary>
        /// <param name="References">正在收集活动正文索引。</param>
        /// <param name="count">捆绑中的正文对数量。</param>
        /// <param name="Orientation">已收集捆绑中正文的方向。</param>
        // [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void GatherOrientation(ref Vector<int> references, int count,
            out QuaternionWide orientation)
        {
            orientation = default;
            Debug.Assert(count >= 0 && count <= Vector<float>.Count);
            // 抓取身体指数的基本引用。请注意,我们再次使用了引用内存布局。
            ref var baseIndexA = ref Unsafe.As<Vector<int>, int>(ref references);

            ref var poses = ref ActiveSet.Poses;
            for (int i = 0; i < count; ++i)
            {
                ref var indexA = ref Unsafe.Add(ref baseIndexA, i);
                QuaternionWide.WriteFirst(poses[indexA].Orientation, ref GatherScatter.GetOffsetInstance(ref orientation, i));
            }
        }



        /// <summary>
        /// 将躯干束的姿势信息收集到AOSOA束中。
        /// </summary>
        /// <param name="References">正在收集活动正文索引。</param>
        /// <param name="count">捆绑中的正文对数量。</param>
        /// <param name="position">已收集正文的绝对位置。</param>
        /// <param name="Orientation">收集的正文方向。</param>
        // [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void GatherPose(ref Vector<int> references, int count, out Vector3Wide position, out QuaternionWide orientation)
        {
            position = default;
            orientation = default;
            // TODO：此函数及其用户(应该相对较少)对于较大的世界位置精度是一个问题。
            // 它直接报告位置,从而以高精度的表示影响向量化逻辑。
            // 您可以重新设计此功能的用户以使其不再需要,但这也有其自身的困难
            // (例如,使抓取马达依赖于用户每帧更新其目标偏移)。
            Debug.Assert(count >= 0 && count <= Vector<float>.Count);
            // 抓取身体指数的基本引用。请注意,我们再次使用了引用内存布局。
            ref var baseIndex = ref Unsafe.As<Vector<int>, int>(ref references);

            ref var poses = ref ActiveSet.Poses;
            for (int i = 0; i < count; ++i)
            {
                ref var indexA = ref Unsafe.Add(ref baseIndex, i);
                ref var poseA = ref poses[indexA];
                Vector3Wide.WriteFirst(poseA.Position, ref GatherScatter.GetOffsetInstance(ref position, i));
                QuaternionWide.WriteFirst(poseA.Orientation, ref GatherScatter.GetOffsetInstance(ref orientation, i));

            }
        }

        /// <summary>
        /// 将两体束的方向和相对位置收集到AOSOA束中。
        /// </summary>
        /// <param name="References">正在收集活动正文索引。</param>
        /// <param name="count">捆绑中的正文对数量。</param>
        /// <param name="offsetB">已收集从正文A到正文B的偏移</param>
        /// <param name="OrientationA">收集的正文A的方向</param>
        /// <param name="OrientationB">收集的正文B的方向</param>
        // [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void GatherPose(ref TwoBodyReferences references, int count,
            out Vector3Wide offsetB, out QuaternionWide orientationA, out QuaternionWide orientationB)
        {
            orientationA = default; orientationB = default;
            Debug.Assert(count >= 0 && count <= Vector<float>.Count);
            // 抓取身体指数的基本引用。请注意,我们再次使用了引用内存布局。
            ref var baseIndexA = ref Unsafe.As<Vector<int>, int>(ref references.IndexA);
            ref var baseIndexB = ref Unsafe.As<Vector<int>, int>(ref references.IndexB);

            Vector3Wide positionA = default, positionB = default;
            ref var poses = ref ActiveSet.Poses;
            for (int i = 0; i < count; ++i)
            {
                ref var indexA = ref Unsafe.Add(ref baseIndexA, i);
                ref var poseA = ref poses[indexA];
                Vector3Wide.WriteFirst(poseA.Position, ref GatherScatter.GetOffsetInstance(ref positionA, i));
                QuaternionWide.WriteFirst(poseA.Orientation, ref GatherScatter.GetOffsetInstance(ref orientationA, i));

                ref var indexB = ref Unsafe.Add(ref baseIndexB, i);
                ref var poseB = ref poses[indexB];
                Vector3Wide.WriteFirst(poseB.Position, ref GatherScatter.GetOffsetInstance(ref positionB, i));
                QuaternionWide.WriteFirst(poseB.Orientation, ref GatherScatter.GetOffsetInstance(ref orientationB, i));
            }
            // TODO：在未来的版本中,我们可能会以不同的形式存储身体位置,以支持极大的世界。
            // 这将是一个选择加入的功能。默认实现将使用fp32表示,但用户可以选择将其替换为fp64或fixed64表示。
            // 这会影响其他系统-AABB计算、姿态积分、求解,在极端(64位)情况下,还会影响宽相位。
            // 我们希望在可能的情况下,将其他系统与有关仓位实施的直接知识隔离开来。
            // 这些函数支持求解器的需要,同时隐藏绝对位置。
            // 为了支持其他绝对位置,我们需要此函数和其他函数的替代实现。
            // 但在很大程度上,我们不想在解算器的内部循环中支付抽象调用的开销。
            // 考虑到当前C#和编译器的限制,最佳选择似乎是条件编译。
            Vector3Wide.Subtract(positionB, positionA, out offsetB);
        }

        /// <summary>
        /// 将两个主体捆绑包的相对位置收集到AOSOA捆绑包中。
        /// </summary>
        /// <param name="References">正在收集活动正文索引。</param>
        /// <param name="count">捆绑中的正文对数量。</param>
        /// <param name="ab">从正文A到正文B的累计偏移量</param>
        // [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void GatherOffsets(ref TwoBodyReferences references, int count, out Vector3Wide ab)
        {
            Debug.Assert(count >= 0 && count <= Vector<float>.Count);
            // 抓取身体指数的基本引用。请注意,我们再次使用了引用内存布局。
            ref var baseIndexA = ref Unsafe.As<Vector<int>, int>(ref references.IndexA);
            ref var baseIndexB = ref Unsafe.As<Vector<int>, int>(ref references.IndexB);

            Vector3Wide positionA = default, positionB = default;
            ref var poses = ref ActiveSet.Poses;
            for (int i = 0; i < count; ++i)
            {
                Vector3Wide.WriteFirst(poses[Unsafe.Add(ref baseIndexA, i)].Position, ref GatherScatter.GetOffsetInstance(ref positionA, i));
                Vector3Wide.WriteFirst(poses[Unsafe.Add(ref baseIndexB, i)].Position, ref GatherScatter.GetOffsetInstance(ref positionB, i));
            }
            // 与其他聚集情况相同;这对身体位置表示的更改很敏感。在高精度模式下,这种情况需要改变。
            Vector3Wide.Subtract(positionB, positionA, out ab);
        }

        /// <summary>
        /// 将三体捆绑包的相对位置收集到AOSOA捆绑包中。
        /// </summary>
        /// <param name="References">正在收集活动正文索引。</param>
        /// <param name="count">捆绑中的正文对数量。</param>
        /// <param name="ab">从正文A到正文B的累计偏移量</param>
        /// <param name="ac">从正文A到正文C的偏移量已汇总</param>
        // [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void GatherOffsets(ref ThreeBodyReferences references, int count, out Vector3Wide ab, out Vector3Wide ac)
        {
            Debug.Assert(count >= 0 && count <= Vector<float>.Count);
            // 抓取身体指数的基本引用。请注意,我们再次使用了引用内存布局。
            ref var baseIndexA = ref Unsafe.As<Vector<int>, int>(ref references.IndexA);
            ref var baseIndexB = ref Unsafe.As<Vector<int>, int>(ref references.IndexB);
            ref var baseIndexC = ref Unsafe.As<Vector<int>, int>(ref references.IndexC);

            Vector3Wide positionA = default, positionB = default, positionC = default;
            ref var poses = ref ActiveSet.Poses;
            for (int i = 0; i < count; ++i)
            {
                Vector3Wide.WriteFirst(poses[Unsafe.Add(ref baseIndexA, i)].Position, ref GatherScatter.GetOffsetInstance(ref positionA, i));
                Vector3Wide.WriteFirst(poses[Unsafe.Add(ref baseIndexB, i)].Position, ref GatherScatter.GetOffsetInstance(ref positionB, i));
                Vector3Wide.WriteFirst(poses[Unsafe.Add(ref baseIndexC, i)].Position, ref GatherScatter.GetOffsetInstance(ref positionC, i));
            }
            // 与两个身体情况相同;这对身体位置表示的变化很敏感。在高精度模式下,这种情况需要改变。
            Vector3Wide.Subtract(positionB, positionA, out ab);
            Vector3Wide.Subtract(positionC, positionA, out ac);
        }
        /// <summary>
        /// 将四体束的相对位置收集到AOSOA束中。
        /// </summary>
        /// <param name="References">正在收集活动正文索引。</param>
        /// <param name="count">捆绑中的正文对数量。</param>
        /// <param name="ab">从正文A到正文B的累计偏移量</param>
        /// <param name="ac">从正文A到正文C的偏移量已汇总</param>
        /// <param name="ad">从正文A到正文D的偏移量已汇总</param>
        // [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void GatherOffsets(ref FourBodyReferences references, int count, out Vector3Wide ab, out Vector3Wide ac, out Vector3Wide ad)
        {
            Debug.Assert(count >= 0 && count <= Vector<float>.Count);
            // 抓取身体指数的基本引用。请注意,我们再次使用了引用内存布局。
            ref var baseIndexA = ref Unsafe.As<Vector<int>, int>(ref references.IndexA);
            ref var baseIndexB = ref Unsafe.As<Vector<int>, int>(ref references.IndexB);
            ref var baseIndexC = ref Unsafe.As<Vector<int>, int>(ref references.IndexC);
            ref var baseIndexD = ref Unsafe.As<Vector<int>, int>(ref references.IndexD);

            Vector3Wide positionA = default, positionB = default, positionC = default, positionD = default;
            ref var poses = ref ActiveSet.Poses;
            for (int i = 0; i < count; ++i)
            {
                Vector3Wide.WriteFirst(poses[Unsafe.Add(ref baseIndexA, i)].Position, ref GatherScatter.GetOffsetInstance(ref positionA, i));
                Vector3Wide.WriteFirst(poses[Unsafe.Add(ref baseIndexB, i)].Position, ref GatherScatter.GetOffsetInstance(ref positionB, i));
                Vector3Wide.WriteFirst(poses[Unsafe.Add(ref baseIndexC, i)].Position, ref GatherScatter.GetOffsetInstance(ref positionC, i));
                Vector3Wide.WriteFirst(poses[Unsafe.Add(ref baseIndexD, i)].Position, ref GatherScatter.GetOffsetInstance(ref positionD, i));
            }
            // 与两个身体情况相同;这对身体位置表示的变化很敏感。在高精度模式下,这种情况需要改变。
            Vector3Wide.Subtract(positionB, positionA, out ab);
            Vector3Wide.Subtract(positionC, positionA, out ac);
            Vector3Wide.Subtract(positionD, positionA, out ad);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static unsafe void GatherVelocities(ref Buffer<BodyVelocity> sources, ref BodyVelocities target, ref int baseIndex, int innerIndex)
        {
            ref var targetSlot = ref GatherScatter.GetOffsetInstance(ref target, innerIndex);
            ref var source = ref sources[Unsafe.Add(ref baseIndex, innerIndex)];
            //GatherScatter.GetFirst(ref targetSlot.Linear.X) = source.Linear.X;
            //GatherScatter.GetFirst(ref targetSlot.Linear.Y) = source.Linear.Y;
            //GatherScatter.GetFirst(ref targetSlot.Linear.Z) = source.Linear.Z;
            //GatherScatter.GetFirst(ref targetSlot.Angular.X) = source.Angular.X;
            //GatherScatter.GetFirst(ref targetSlot.Angular.Y) = source.Angular.Y;
            //GatherScatter.GetFirst(ref targetSlot.Angular.Z) = source.Angular.Z;

            GatherScatter.SetFirst(ref targetSlot.Linear.X, source.Linear.X);
            GatherScatter.SetFirst(ref targetSlot.Linear.Y, source.Linear.Y);
            GatherScatter.SetFirst(ref targetSlot.Linear.Z, source.Linear.Z);
            GatherScatter.SetFirst(ref targetSlot.Angular.X, source.Angular.X);
            GatherScatter.SetFirst(ref targetSlot.Angular.Y, source.Angular.Y);
            GatherScatter.SetFirst(ref targetSlot.Angular.Z, source.Angular.Z);
        }

        /// <summary>
        /// 收集一个躯干束的速度并将其存储到速度束中。
        /// </summary>
        /// <param name="reference">要为其收集速度数据的实体的活动集索引。</param>
        /// <param name="count">捆绑中的正文数。</param>
        /// <param name="veloities">收集的速度。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static unsafe void GatherVelocities(ref Buffer<BodyVelocity> sourceVelocities, ref Vector<int> references, int count, out BodyVelocities velocities)
        {
            velocities = default;
            // 抓取身体指数的基本引用。请注意,我们再次使用了引用内存布局。
            ref var baseIndex = ref Unsafe.As<Vector<int>, int>(ref references);
            for (int i = 0; i < count; ++i)
            {
                GatherVelocities(ref sourceVelocities, ref velocities, ref baseIndex, i);
            }
        }

        /// <summary>
        /// 收集两个躯干束的速度并将其存储到速度束中。
        /// </summary>
        /// <param name="reference">要为其收集速度数据的实体的活动集索引。</param>
        /// <param name="count">捆绑中的正文对数量。</param>
        /// <param name="velocitiesA">收集的A形体速度。</param>
        /// <param name="velocitiesB">收集的B形体的速度。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static unsafe void GatherVelocities(ref Buffer<BodyVelocity> sourceVelocities, ref TwoBodyReferences references, int count, out BodyVelocities velocitiesA, out BodyVelocities velocitiesB)
        {
            velocitiesA = default; velocitiesB = default;
            Debug.Assert(count >= 0 && count <= Vector<float>.Count);
            // 抓取身体指数的基本引用。请注意,我们再次使用了引用内存布局。
            ref var baseIndexA = ref Unsafe.As<Vector<int>, int>(ref references.IndexA);
            ref var baseIndexB = ref Unsafe.As<Vector<int>, int>(ref references.IndexB);
            for (int i = 0; i < count; ++i)
            {
                GatherVelocities(ref sourceVelocities, ref velocitiesA, ref baseIndexA, i);
                GatherVelocities(ref sourceVelocities, ref velocitiesB, ref baseIndexB, i);
            }
        }

        /// <summary>
        /// 收集三个躯干束的速度并将其存储到速度束中。
        /// </summary>
        /// <param name="reference">要为其收集速度数据的实体的活动集索引。</param>
        /// <param name="count">捆绑中的正文对数量。</param>
        /// <param name="velocitiesA">收集的A形体速度。</param>
        /// <param name="velocitiesB">收集的B形体的速度。</param>
        /// <param name="velocitiesC">收集的C形体速度。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static unsafe void GatherVelocities(ref Buffer<BodyVelocity> sourceVelocities, ref ThreeBodyReferences references, int count,
            out BodyVelocities velocitiesA, out BodyVelocities velocitiesB, out BodyVelocities velocitiesC)
        {
            velocitiesA = default; velocitiesB = default; velocitiesC = default;
            Debug.Assert(count >= 0 && count <= Vector<float>.Count);
            // 抓取身体指数的基本引用。请注意,我们再次使用了引用内存布局。
            ref var baseIndexA = ref Unsafe.As<Vector<int>, int>(ref references.IndexA);
            ref var baseIndexB = ref Unsafe.As<Vector<int>, int>(ref references.IndexB);
            ref var baseIndexC = ref Unsafe.As<Vector<int>, int>(ref references.IndexC);
            for (int i = 0; i < count; ++i)
            {
                GatherVelocities(ref sourceVelocities, ref velocitiesA, ref baseIndexA, i);
                GatherVelocities(ref sourceVelocities, ref velocitiesB, ref baseIndexB, i);
                GatherVelocities(ref sourceVelocities, ref velocitiesC, ref baseIndexC, i);
            }
        }

        /// <summary>
        /// 收集四个躯干束的速度并将其存储到速度束中。
        /// </summary>
        /// <param name="reference">要为其收集速度数据的实体的活动集索引。</param>
        /// <param name="count">捆绑中的正文对数量。</param>
        /// <param name="velocitiesA">收集的A形体速度。</param>
        /// <param name="velocitiesB">收集的B形体的速度。</param>
        /// <param name="velocitiesC">收集的C形体速度。</param>
        /// <param name="velocitiesD">收集的D形体速度。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static unsafe void GatherVelocities(ref Buffer<BodyVelocity> sourceVelocities, ref FourBodyReferences references, int count,
            out BodyVelocities velocitiesA, out BodyVelocities velocitiesB, out BodyVelocities velocitiesC, out BodyVelocities velocitiesD)
        {
            velocitiesA = default; velocitiesB = default; velocitiesC = default; velocitiesD = default;
            Debug.Assert(count >= 0 && count <= Vector<float>.Count);
            // 抓取身体指数的基本引用。请注意,我们再次使用了引用内存布局。
            ref var baseIndexA = ref Unsafe.As<Vector<int>, int>(ref references.IndexA);
            ref var baseIndexB = ref Unsafe.As<Vector<int>, int>(ref references.IndexB);
            ref var baseIndexC = ref Unsafe.As<Vector<int>, int>(ref references.IndexC);
            ref var baseIndexD = ref Unsafe.As<Vector<int>, int>(ref references.IndexD);
            for (int i = 0; i < count; ++i)
            {
                GatherVelocities(ref sourceVelocities, ref velocitiesA, ref baseIndexA, i);
                GatherVelocities(ref sourceVelocities, ref velocitiesB, ref baseIndexB, i);
                GatherVelocities(ref sourceVelocities, ref velocitiesC, ref baseIndexC, i);
                GatherVelocities(ref sourceVelocities, ref velocitiesD, ref baseIndexD, i);
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static unsafe void ScatterVelocities(ref BodyVelocities sourceVelocities, ref Buffer<BodyVelocity> targets, ref int baseIndex, int innerIndex)
        {
            // TODO：在运动状态上分支并避免写入有多大价值？这在很大程度上取决于运动学的数量。
            ref var sourceSlot = ref GatherScatter.GetOffsetInstance(ref sourceVelocities, innerIndex);
            ref var target = ref targets[Unsafe.Add(ref baseIndex, innerIndex)];
            //target.Linear.X = GatherScatter.GetFirst(ref sourceSlot.Linear.X);
            //target.Linear.Y = GatherScatter.GetFirst(ref sourceSlot.Linear.Y);
            //target.Linear.Z = GatherScatter.GetFirst(ref sourceSlot.Linear.Z);
            //target.Angular.X = GatherScatter.GetFirst(ref sourceSlot.Angular.X);
            //target.Angular.Y = GatherScatter.GetFirst(ref sourceSlot.Angular.Y);
            //target.Angular.Z = GatherScatter.GetFirst(ref sourceSlot.Angular.Z);

            target.Linear.X = sourceSlot.Linear.X[0];
            target.Linear.Y = sourceSlot.Linear.Y[0];
            target.Linear.Z = sourceSlot.Linear.Z[0];
            target.Angular.X = sourceSlot.Angular.X[0];
            target.Angular.Y = sourceSlot.Angular.Y[0];
            target.Angular.Z = sourceSlot.Angular.Z[0];
        }

        /// <summary>
        /// 将一个躯干束的速度散布到活动躯干集中。
        /// </summary>
        /// <param name="sourceVeloities">要分散的正文束A的速度。</param>
        /// <param name="reference">要向其散布速度数据的实体的活动集索引。</param>
        /// <param name="count">捆绑中的正文对数量。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static unsafe void ScatterVelocities(ref BodyVelocities sourceVelocities, ref Buffer<BodyVelocity> targetVelocities, ref Vector<int> references, int count)
        {
            Debug.Assert(count >= 0 && count <= Vector<float>.Count);
            // 抓取身体指数的基本引用。请注意,我们再次使用了引用内存布局。
            ref var baseIndex = ref Unsafe.As<Vector<int>, int>(ref references);
            for (int i = 0; i < count; ++i)
            {
                ScatterVelocities(ref sourceVelocities, ref targetVelocities, ref baseIndex, i);
            }
        }

        /// <summary>
        /// 将两个躯干束的速度散布到活动实体集中。
        /// </summary>
        /// <param name="sourceVelocitiesA">要分散的主体束A的速度。</param>
        /// <param name="sourceVelocitiesA">要分散的正文束B的速度。</param>
        /// <param name="reference">要向其散布速度数据的实体的活动集索引。</param>
        /// <param name="count">捆绑中的正文对数量。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static unsafe void ScatterVelocities(ref BodyVelocities sourceVelocitiesA, ref BodyVelocities sourceVelocitiesB, ref Buffer<BodyVelocity> targetVelocities,
            ref TwoBodyReferences references, int count)
        {
            Debug.Assert(count >= 0 && count <= Vector<float>.Count);
            // 抓取身体指数的基本引用。请注意,我们再次使用了引用内存布局。
            ref var baseIndexA = ref Unsafe.As<Vector<int>, int>(ref references.IndexA);
            ref var baseIndexB = ref Unsafe.As<Vector<int>, int>(ref references.IndexB);
            for (int i = 0; i < count; ++i)
            {
                ScatterVelocities(ref sourceVelocitiesA, ref targetVelocities, ref baseIndexA, i);
                ScatterVelocities(ref sourceVelocitiesB, ref targetVelocities, ref baseIndexB, i);
            }
        }

        /// <summary>
        /// 将三个躯干束的速度散布到活动实体集中。
        /// </summary>
        /// <param name="sourceVelocitiesA">要分散的主体束A的速度。</param>
        /// <param name="sourceVelocitiesB">要散布的正文束B的速度。</param>
        /// <param name="sourceVelocitiesC">要分散的主体束A的速度。</param>
        /// <param name="reference">要向其散布速度数据的实体的活动集索引。</param>
        /// <param name="count">捆绑中的正文对数量。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static unsafe void ScatterVelocities(
            ref BodyVelocities sourceVelocitiesA, ref BodyVelocities sourceVelocitiesB, ref BodyVelocities sourceVelocitiesC,
            ref Buffer<BodyVelocity> targetVelocities, ref ThreeBodyReferences references, int count)
        {
            Debug.Assert(count >= 0 && count <= Vector<float>.Count);
            // 抓取身体指数的基本引用。请注意,我们再次使用了引用内存布局。
            ref var baseIndexA = ref Unsafe.As<Vector<int>, int>(ref references.IndexA);
            ref var baseIndexB = ref Unsafe.As<Vector<int>, int>(ref references.IndexB);
            ref var baseIndexC = ref Unsafe.As<Vector<int>, int>(ref references.IndexC);
            for (int i = 0; i < count; ++i)
            {
                ScatterVelocities(ref sourceVelocitiesA, ref targetVelocities, ref baseIndexA, i);
                ScatterVelocities(ref sourceVelocitiesB, ref targetVelocities, ref baseIndexB, i);
                ScatterVelocities(ref sourceVelocitiesC, ref targetVelocities, ref baseIndexC, i);
            }
        }

        /// <summary>
        /// 将四个躯干束的速度散布到活动实体集中。
        /// </summary>
        /// <param name="sourceVelocitiesA">要分散的主体束A的速度。</param>
        /// <param name="sourceVelocitiesB">要散布的正文束B的速度。</param>
        /// <param name="sourceVelocitiesC">要分散的主体束A的速度。</param>
        /// <param name="sourceVelocitiesD">要分散的正文束B的速度。</param>
        /// <param name="reference">要向其散布速度数据的实体的活动集索引。</param>
        /// <param name="count">捆绑中的正文对数量。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static unsafe void ScatterVelocities(
            ref BodyVelocities sourceVelocitiesA, ref BodyVelocities sourceVelocitiesB, ref BodyVelocities sourceVelocitiesC, ref BodyVelocities sourceVelocitiesD,
            ref Buffer<BodyVelocity> targetVelocities, ref FourBodyReferences references, int count)
        {
            Debug.Assert(count >= 0 && count <= Vector<float>.Count);
            // 抓取身体指数的基本引用。请注意,我们再次使用了引用内存布局。
            ref var baseIndexA = ref Unsafe.As<Vector<int>, int>(ref references.IndexA);
            ref var baseIndexB = ref Unsafe.As<Vector<int>, int>(ref references.IndexB);
            ref var baseIndexC = ref Unsafe.As<Vector<int>, int>(ref references.IndexC);
            ref var baseIndexD = ref Unsafe.As<Vector<int>, int>(ref references.IndexD);
            for (int i = 0; i < count; ++i)
            {
                ScatterVelocities(ref sourceVelocitiesA, ref targetVelocities, ref baseIndexA, i);
                ScatterVelocities(ref sourceVelocitiesB, ref targetVelocities, ref baseIndexB, i);
                ScatterVelocities(ref sourceVelocitiesC, ref targetVelocities, ref baseIndexC, i);
                ScatterVelocities(ref sourceVelocitiesD, ref targetVelocities, ref baseIndexD, i);
            }
        }

        internal void ResizeSetsCapacity(int setsCapacity, int potentiallyAllocatedCount)
        {
            Debug.Assert(setsCapacity >= potentiallyAllocatedCount && potentiallyAllocatedCount <= Sets.Length);
            setsCapacity = BufferPool.GetCapacityForCount<BodySet>(setsCapacity);
            if (Sets.Length != setsCapacity)
            {
                var oldCapacity = Sets.Length;
                Pool.ResizeToAtLeast(ref Sets, setsCapacity, potentiallyAllocatedCount);
                if (oldCapacity < Sets.Length)
                    Sets.Clear(oldCapacity, Sets.Length - oldCapacity); // 我们依赖于默认初始化未使用的插槽。
            }
        }

        struct ActiveConstraintBodyIndicesEnumerator<TInnerEnumerator> : IForEach<int> where TInnerEnumerator : IForEach<int>
        {
            public TInnerEnumerator InnerEnumerator;
            public int SourceBodyIndex;

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public void LoopBody(int connectedBodyIndex)
            {
                if (SourceBodyIndex != connectedBodyIndex)
                {
                    // 请注意,如果多次连接,这可能会多次报告同一正文！这很好,而且有潜在的用处;让用户来处理吧。
                    InnerEnumerator.LoopBody(connectedBodyIndex);
                }
            }

        }
        struct ActiveConstraintBodyHandleEnumerator<TInnerEnumerator> : IForEach<int> where TInnerEnumerator : IForEach<BodyHandle>
        {
            public Bodies bodies;
            public TInnerEnumerator InnerEnumerator;
            public int SourceBodyIndex;

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public void LoopBody(int connectedBodyIndex)
            {
                if (SourceBodyIndex != connectedBodyIndex)
                {
                    // 此枚举器与公共连接团体枚举器函数相关联。用户提供一个句柄,并期望返回句柄,因此我们
                    // 必须将求解器提供的索引转换为句柄。
                    InnerEnumerator.LoopBody(bodies.ActiveSet.IndexToHandle[connectedBodyIndex]);
                }
            }

        }
        // 请注意,非活动约束按控制柄而不是索引引用实体。
        // 但是,求解器通过整数值而不是类型化的BodyHandles来存储它们。有点恶心,不过我们可以在这里翻译。
        struct InactiveConstraintBodyHandleEnumerator<TInnerEnumerator> : IForEach<int> where TInnerEnumerator : IForEach<BodyHandle>
        {
            public TInnerEnumerator InnerEnumerator;
            public BodyHandle SourceBodyHandle;

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public void LoopBody(int connectedBodyHandle)
            {
                if (SourceBodyHandle.Value != connectedBodyHandle)
                {
                    // 由于此枚举器与直接存储正文句柄而不是正文索引的非活动约束相关联,
                    // 我们可以直接传递求解器提供的句柄。
                    InnerEnumerator.LoopBody(new BodyHandle(connectedBodyHandle));
                }
            }

        }

        /// <summary>
        /// 枚举连接到给定活动实体的所有实体。
        /// 通过多个约束连接的实体将多次报告。
        /// </summary>
        /// <typeparam name="TEnumerator">要在每个连接的正文上执行的枚举数的类型。</typeparam>
        /// <param name="activeBodyIndex">要枚举其连接的活动主体的索引。此正文不会出现在枚举正文集中,即使它以某种方式与自身连接。</param>
        /// <param name="Enumererator">要在每个连接的正文上运行的枚举器实例。</param>
        /// <param name="solver">从中提取约束正文引用的求解器。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        internal void EnumerateConnectedBodyIndices<TEnumerator>(int activeBodyIndex, ref TEnumerator enumerator) where TEnumerator : IForEach<int>
        {
            ref var list = ref ActiveSet.Constraints[activeBodyIndex];
            ActiveConstraintBodyIndicesEnumerator<TEnumerator> constraintBodiesEnumerator;
            constraintBodiesEnumerator.InnerEnumerator = enumerator;
            constraintBodiesEnumerator.SourceBodyIndex = activeBodyIndex;

            // 注意反向迭代。这在执行O(1)删除(将最后一个元素放入已删除元素的位置)时非常有用。
            // 如果循环体删除了任何内容,则非反向迭代将导致跳过元素。这依赖于约定;任何移除器都应该知道这个顺序。
            for (int i = list.Count - 1; i >= 0; --i)
            {
                solver.EnumerateConnectedBodies(list[i].ConnectingConstraintHandle, ref constraintBodiesEnumerator);
            }
            // 请注意,我们必须假设枚举器包含由内部循环体改变的状态。
            // 如果它是值类型,这些突变将不会反映在原始引用中。
            // 把它们复制回来。
            enumerator = constraintBodiesEnumerator.InnerEnumerator;
        }
        /// <summary>
        /// 枚举连接到给定实体的所有实体。
        /// 通过多个约束连接的实体将多次报告。
        /// </summary>
        /// <typeparam name="TEnumerator">要在每个连接的正文上执行的枚举数的类型。</typeparam>
        /// 要枚举其连接的正文的<param name="bodyHandle">句柄。此正文不会出现在枚举正文集中,即使它以某种方式与自身连接。</param>
        /// <param name="Enumererator">要在每个连接的正文上运行的枚举器实例。</param>
        /// <param name="solver">从中提取约束正文引用的求解器。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void EnumerateConnectedBodies<TEnumerator>(BodyHandle bodyHandle, ref TEnumerator enumerator) where TEnumerator : IForEach<BodyHandle>
        {
            ref var bodyLocation = ref HandleToLocation[bodyHandle.Value];
            ref var set = ref Sets[bodyLocation.SetIndex];
            ref var list = ref set.Constraints[bodyLocation.Index];
            // 在下面的循环中,我们仍然使用反向迭代。从枚举数上下文中移除是一种危险的移动,但如果用户
            // 是很小心的。通过在所有这些枚举中维护相同的约定,可以更容易地进行可靠的操作。
            if (bodyLocation.SetIndex == 0)
            {
                // 该实体处于活动状态。使用活动枚举器。
                ActiveConstraintBodyHandleEnumerator<TEnumerator> constraintBodiesEnumerator;
                constraintBodiesEnumerator.InnerEnumerator = enumerator;
                constraintBodiesEnumerator.SourceBodyIndex = bodyLocation.Index;
                constraintBodiesEnumerator.bodies = this;

                for (int i = list.Count - 1; i >= 0; --i)
                {
                    solver.EnumerateConnectedBodies(list[i].ConnectingConstraintHandle, ref constraintBodiesEnumerator);
                }
                enumerator = constraintBodiesEnumerator.InnerEnumerator;
            }
            else
            {
                // 身体是不活动的。使用非活动枚举器。
                InactiveConstraintBodyHandleEnumerator<TEnumerator> constraintBodiesEnumerator;
                constraintBodiesEnumerator.InnerEnumerator = enumerator;
                constraintBodiesEnumerator.SourceBodyHandle = bodyHandle;

                for (int i = list.Count - 1; i >= 0; --i)
                {
                    solver.EnumerateConnectedBodies(list[i].ConnectingConstraintHandle, ref constraintBodiesEnumerator);
                }
                enumerator = constraintBodiesEnumerator.InnerEnumerator;
            }

        }
        /// <summary>
        /// 从所有集合中清除所有实体,而不释放任何不会通过常规移除序列释放的内存。
        /// </summary>
        public unsafe void Clear()
        {
            ActiveSet.Clear(Pool);
            // 虽然顶层池代表活动主体并且将持续存在(就像在一系列移除之后一样),
            // 后续集合表示不活动的实体。当它们不存在时,释放后备存储器。
            for (int i = 1; i < Sets.Length; ++i)
            {
                ref var set = ref Sets[i];
                if (set.Allocated)
                    set.Dispose(Pool);
            }
            Unsafe.InitBlockUnaligned(HandleToLocation.Memory, 0xFF, (uint)(sizeof(BodyMemoryLocation) * HandleToLocation.Length));
            HandlePool.Clear();
        }


        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        unsafe void ResizeHandles(int newCapacity)
        {
            newCapacity = BufferPool.GetCapacityForCount<BodyMemoryLocation>(newCapacity);
            if (newCapacity != HandleToLocation.Length)
            {
                var oldCapacity = HandleToLocation.Length;
                Pool.ResizeToAtLeast(ref HandleToLocation, newCapacity, Math.Min(oldCapacity, newCapacity));
                if (HandleToLocation.Length > oldCapacity)
                {
                    Unsafe.InitBlockUnaligned(
                      HandleToLocation.Memory + oldCapacity, 0xFF,
                      (uint)(sizeof(BodyMemoryLocation) * (HandleToLocation.Length - oldCapacity)));
                }
            }
        }
        // 请注意,这些调整大小并确保容量功能仅影响活动集。
        // 非活动孤岛是以最少的分配创建的。由于您不能向非活动岛屿添加或从中删除,因此尝试修改它们的分配大小是没有意义的。
        /// <summary>
        /// 为目标容量重新分配惯性缓冲区。不会缩小到当前活动集的大小以下。
        /// </summary>
        internal void ResizeInertias(int capacity)
        {
            var targetCapacity = BufferPool.GetCapacityForCount<BodyInertia>(Math.Max(capacity, ActiveSet.Count));
            if (Inertias.Length != targetCapacity)
            {
                Pool.ResizeToAtLeast(ref Inertias, targetCapacity, Math.Min(Inertias.Length, ActiveSet.Count));
            }
        }
        /// <summary>
        /// 保证惯性能力对于给定的能力是足够的。
        /// </summary>
        internal void EnsureInertiasCapacity(int capacity)
        {
            if (capacity < ActiveSet.Count)
                capacity = ActiveSet.Count;
            if (Inertias.Length < capacity)
            {
                Pool.ResizeToAtLeast(ref Inertias, capacity, Math.Min(Inertias.Length, ActiveSet.Count));
            }
        }

        /// <summary>
        /// 调整为活动实体数据分配的跨度的大小。请注意,这是保守的;它永远不会孤立现有对象。
        /// </summary>
        /// <param name="Capacity">目标正文数据容量。</param>
        public void Resize(int capacity)
        {
            var targetBodyCapacity = BufferPool.GetCapacityForCount<int>(Math.Max(capacity, ActiveSet.Count));
            if (ActiveSet.IndexToHandle.Length != targetBodyCapacity)
            {
                ActiveSet.InternalResize(targetBodyCapacity, Pool);
            }
            ResizeInertias(capacity);
            var targetHandleCapacity = BufferPool.GetCapacityForCount<int>(Math.Max(capacity, HandlePool.HighestPossiblyClaimedId + 1));
            if (HandleToLocation.Length != targetHandleCapacity)
            {
                ResizeHandles(targetHandleCapacity);
            }
        }

        /// <summary>
        /// 调整所有活动实体约束列表的大小以满足MinimumConstraintCapacityPerBody。不活动的实体未被触及。
        /// 调整大小保证永远不会将列表缩小到当前计数以下。
        /// </summary>
        public void ResizeConstraintListCapacities()
        {
            for (int i = 0; i < ActiveSet.Count; ++i)
            {
                ref var list = ref ActiveSet.Constraints[i];
                var targetCapacity = BufferPool.GetCapacityForCount<BodyConstraintReference>(list.Count > MinimumConstraintCapacityPerBody ? list.Count : MinimumConstraintCapacityPerBody);
                if (list.Span.Length != targetCapacity)
                    list.Resize(targetCapacity, Pool);
            }
        }

        /// <summary>
        /// 如果需要容纳目标容量,则增加活动正文缓冲区的大小。
        /// </summary>
        /// <param name="Capacity">目标数据容量。</param>
        public void EnsureCapacity(int capacity)
        {
            if (ActiveSet.IndexToHandle.Length < capacity)
            {
                ActiveSet.InternalResize(capacity, Pool);
            }
            EnsureInertiasCapacity(capacity);
            if (HandleToLocation.Length < capacity)
            {
                ResizeHandles(capacity);
            }
        }

        /// <summary>
        /// 确保所有活动的Body约束列表可以至少包含MinimumConstraintCapacityPerBody约束。不活动的实体未被触及。
        /// </summary>
        public void EnsureConstraintListCapacities()
        {
            for (int i = 0; i < ActiveSet.Count; ++i)
            {
                ref var list = ref ActiveSet.Constraints[i];
                if (list.Span.Length < MinimumConstraintCapacityPerBody)
                    list.Resize(MinimumConstraintCapacityPerBody, Pool);
            }
        }

        /// <summary>
        /// 将所有正文资源返回到用于创建它们的池。
        /// </summary>
        /// <remarks>The object can be reused if it is reinitialized by using EnsureCapacity or Resize.</remarks>
        public void Dispose()
        {
            for (int i = 0; i < Sets.Length; ++i)
            {
                ref var set = ref Sets[i];
                if (set.Allocated)
                {
                    set.Dispose(Pool);
                }
            }
            Pool.Return(ref Sets);
            if (Inertias.Allocated)
                Pool.Return(ref Inertias);
            Pool.Return(ref HandleToLocation);
            HandlePool.Dispose(Pool);
        }

    }
}
